#include "full_cover_path_planner/bstphdns_planner.h"
#include "path_smooth/path_smoother.h"
#include "glog_func/log.h"

#include <unordered_set>

using namespace global_path_planner::path_smooth;

namespace global_path_planner
{
    namespace ccpp
    {
        BstphdnsPlanner::BstphdnsPlanner(std::shared_ptr<MapProcess>& map_process,
                                        std::shared_ptr<CellDecomposer>& cell_decomp,
                                        std::shared_ptr<Polygon2D>& poly2d)
                                        : __map_Proc_Ptr(map_process),
                                        __polygon2d_Ptr(poly2d),
                                        __cell_Decomp_Ptr(cell_decomp)
        {
            __ccpp_Proc_Ptr = std::make_shared<CCPPPathProcess>(CCPPPathProcess(__polygon2d_Ptr));
        }

        BstphdnsPlanner::~BstphdnsPlanner()
        {

        }

        // 普通弓字形路径
        std::vector<std::vector<Point_2>>
        BstphdnsPlanner::getBoustrophedonPath(Point_2 &start, int &narrow_lane, cv::Mat& img,
                                            float &sweep_width, float &sweep_interval,
                                            bool& arc_boustrophedon, std::string& info,
                                            std::vector<std::vector<cv::Point>> polys)
        {
            std::vector<std::vector<Point_2>> res_pts;
            std::string error;
            // 1.依据轮廓构造几何多边形模型
            auto pwhs = __cell_Decomp_Ptr->CreatePolygonWithHoles(polys);
            if(pwhs.size() == 0)
            {
                info = "The number of structure polygons is 0";
                return res_pts;
            }
            // 2.分解多边形生成单元cell
            auto bcd_cells = __cell_Decomp_Ptr->DecomposePolygons(pwhs, img);
            if (bcd_cells.size() == 0)
            {
                info = "The number of boustrophedons decomposition cells is 0";
                return res_pts;
            } else {
                LOG(INFO) << "bcd_cells' size " << bcd_cells.size();
            }
            // 3.计算cell内路径
            auto cells_sweeps = getSweepPaths(narrow_lane, sweep_width, img, sweep_interval, bcd_cells);
            if (cells_sweeps.size() == 0)
            {
                info = "The number of the cleaning paths is 0";
                return res_pts;
            } else {
                LOG(INFO) << "cells_sweeps' size " << cells_sweeps.size();
            }
            // 4.构造清扫cell顺序
            auto cell_idx_path = sortCellsSweep(cells_sweeps, start);
            if (cell_idx_path.size() == 0)
            {
                info = "The number of reindexed cell index sets is 0";
                return res_pts;
            }
            // 5.优化弓字形 - 圆弧弓字形， TODO: rj.wang 当前需要把__arc_Boustrophedon改成true
            res_pts = getOptimizedBoustrophedonPaths(arc_boustrophedon,
                                                    sweep_width,
                                                    sweep_interval,
                                                    cell_idx_path,
                                                    cells_sweeps);
            return res_pts;
        }

        // 圆弧弓字形路径
        std::vector<std::vector<Point_2>>
        BstphdnsPlanner::getArcBoustrophedonPath(float &sweep_width, float &sweep_interval,
                                                 std::vector<std::vector<Point_2>> &ori_path)
        {
            std::vector<std::vector<Point_2>> res_optimized_path;
            if (ori_path.size() <= 0)
                return res_optimized_path;
            int arc_radius = (sweep_width / sweep_interval) / 2.0;
            // 1/4弧长
            float arc_lenth_4 = (1.0 / 4) * 2 * M_PI * arc_radius;
            // 间隔弧度
            float interval_radian = (M_PI / 2.0) / (std::floor(arc_lenth_4));
            // 增加返回路径
            for (auto i = 0; i < ori_path.size(); i++)
            {
                if (ori_path[i].size() <= 2)
                {
                    res_optimized_path.emplace_back(ori_path[i]);
                    continue;
                }
                std::vector<Point_2> reverse_path;
                std::vector<Point_2> arc_path;
                for (auto j = 0; j < ori_path[i].size(); j += 2)
                {
                    double mid_x_1, mid_y_1, mid_x_2, mid_y_2;
                    auto dy = CGAL::to_double(ori_path[i][j + 1].y()) - CGAL::to_double(ori_path[i][j].y());
                    auto dx = CGAL::to_double(ori_path[i][j + 1].x()) - CGAL::to_double(ori_path[i][j].x());
                    // 源路径结束时增加一段圆弧
                    if (j + 1 == ori_path[i].size() - 1)
                    {
                        mid_x_2 = (CGAL::to_double(ori_path[i][j + 1].x()) + CGAL::to_double(ori_path[i][j - 2].x())) / 2;
                        mid_y_2 = (CGAL::to_double(ori_path[i][j + 1].y()) + CGAL::to_double(ori_path[i][j - 2].y())) / 2;
                        // 相邻两条路径的纵坐标/横坐标的差值x
                        auto adj_dy = CGAL::to_double(ori_path[i][j - 2].y()) - CGAL::to_double(ori_path[i][j + 1].y());
                        auto adj_dx = CGAL::to_double(ori_path[i][j - 2].x()) - CGAL::to_double(ori_path[i][j + 1].x());
                        // 源路径增加圆弧
                        auto result = getOneCellAdjArcPath(std::ceil(arc_lenth_4) / 2, interval_radian, adj_dx, adj_dy, arc_radius, dx, dy, mid_x_2, mid_y_2);
                        arc_path.insert(arc_path.end(), result.begin(), result.end());
                        break;
                    }
                    mid_x_1 = (CGAL::to_double(ori_path[i][j].x()) + CGAL::to_double(ori_path[i][j + 3].x())) / 2;
                    mid_y_1 = (CGAL::to_double(ori_path[i][j].y()) + CGAL::to_double(ori_path[i][j + 3].y())) / 2;
                    mid_x_2 = (CGAL::to_double(ori_path[i][j + 1].x()) + CGAL::to_double(ori_path[i][j + 2].x())) / 2;
                    mid_y_2 = (CGAL::to_double(ori_path[i][j + 1].y()) + CGAL::to_double(ori_path[i][j + 2].y())) / 2;
                    reverse_path.emplace_back(Point_2(mid_x_2, mid_y_2));
                    reverse_path.emplace_back(Point_2(mid_x_1, mid_y_1));
                    // 相邻两条路径的纵坐标/横坐标的差值x
                    auto adj_dy = CGAL::to_double(ori_path[i][j + 2].y()) - CGAL::to_double(ori_path[i][j + 1].y());
                    auto adj_dx = CGAL::to_double(ori_path[i][j + 2].x()) - CGAL::to_double(ori_path[i][j + 1].x());
                    // 源路径增加圆弧
                    auto result = getOneCellAdjArcPath(std::ceil(arc_lenth_4), interval_radian, adj_dx, adj_dy, arc_radius, dx, dy, mid_x_2, mid_y_2);
                    arc_path.insert(arc_path.end(), result.begin(), result.end());
                }

                std::reverse(reverse_path.begin(), reverse_path.end());
                // 返回路径增加圆弧
                std::vector<Point_2> reverse_arc_path;
                for (auto j = 0; j < reverse_path.size(); j += 2)
                {
                    int mid_x_1, mid_y_1, mid_x_2, mid_y_2;
                    if (j + 1 == reverse_path.size() - 1)
                        break;
                    mid_x_1 = (CGAL::to_double(reverse_path[j].x()) + CGAL::to_double(reverse_path[j + 3].x())) / 2;
                    mid_y_1 = (CGAL::to_double(reverse_path[j].y()) + CGAL::to_double(reverse_path[j + 3].y())) / 2;
                    mid_x_2 = (CGAL::to_double(reverse_path[j + 1].x()) + CGAL::to_double(reverse_path[j + 2].x())) / 2;
                    mid_y_2 = (CGAL::to_double(reverse_path[j + 1].y()) + CGAL::to_double(reverse_path[j + 2].y())) / 2;
                    auto dy = CGAL::to_double(reverse_path[j + 1].y()) - CGAL::to_double(reverse_path[j].y());
                    auto dx = CGAL::to_double(reverse_path[j + 1].x()) - CGAL::to_double(reverse_path[j].x());

                    // 相邻两条路径的纵坐标/横坐标的差值x
                    auto adj_dy = CGAL::to_double(reverse_path[j + 2].y()) - CGAL::to_double(reverse_path[j + 1].y());
                    auto adj_dx = CGAL::to_double(reverse_path[j + 2].x()) - CGAL::to_double(reverse_path[j + 1].x());
                    auto result = getOneCellAdjArcPath(std::ceil(arc_lenth_4), interval_radian, adj_dx, adj_dy, arc_radius, dx, dy, mid_x_2, mid_y_2);
                    reverse_arc_path.insert(reverse_arc_path.end(), result.begin(), result.end());
                }
                arc_path.insert(arc_path.begin(), ori_path[i].front());
                reverse_arc_path.insert(reverse_arc_path.end(), reverse_path.back());
                std::vector<Point_2> singal_res_path;
                singal_res_path.insert(singal_res_path.end(), arc_path.begin(), arc_path.end());
                singal_res_path.insert(singal_res_path.end(), reverse_arc_path.begin(), reverse_arc_path.end());
                res_optimized_path.emplace_back(singal_res_path);
            }
            return res_optimized_path;
        }

        // 计算cells清扫路径
        std::vector<std::vector<Point_2>> BstphdnsPlanner::getSweepPaths(int &narrow_lane, float &sweep_width, cv::Mat& img,
                                                                         float &sweep_interval, std::vector<Polygon_2> &bcd_cells)
        {
            std::vector<std::vector<Point_2>> cells_sweeps;
            std::string errorInfo;
            float sweep_step = sweep_width / sweep_interval; // sweep_interval: 地图栅格分辨率
            if (narrow_lane == 1)
                sweep_step = 0.2 / sweep_interval;
            for (auto it = bcd_cells.begin(); it < bcd_cells.end();)
            {
                // Compute all cluster sweeps.
                std::vector<std::vector<Point_2>> cell_sweeps;
                // 构造最小外接多边形的方向
                // auto cv_dir = __cell_Decomp_Ptr->getlongestEdge(*it);
                // Direction_2 best_dir(Line_2(Point_2(std::get<0>(cv_dir).x, std::get<0>(cv_dir).y),
                //                             Point_2(std::get<1>(cv_dir).x, std::get<1>(cv_dir).y)));
                Direction_2 best_dir = __cell_Decomp_Ptr->findBestDirForSweep(*it);
                // PolygonWithHoles pwh_it(*it);
                // Direction_2 best_dir = __cell_Decomp_Ptr->findBestDirForDecomposer(pwh_it);

                cell_sweeps = calcCellSweepPath(*it, sweep_step, best_dir);
                // auto uniform_sweep_cell = polygon_coverage_planning::genUniformSweepPath(bcd_cells[i], cell_sweep, sweep_step);
                // cell_sweep = uniform_sweep_cell;
                if (cell_sweeps.size() > 0)
                {
                    for (auto i = 0; i < cell_sweeps.size(); i++)
                    {
                        cells_sweeps.emplace_back(cell_sweeps[i]);
                    }
                    it++;
                }
                else
                {
                    it = bcd_cells.erase(it);
                }
            }
            // 移除单个cell中重复路径
            for (auto i = 0; i < cells_sweeps.size(); i++)
            {
                __ccpp_Proc_Ptr->removeCellPathsLessSweepRadius(cells_sweeps[i], sweep_step / 2.0);
            }
            // 去除重排后cell之间的重复路径
            if (cells_sweeps.size() > 1)
                __ccpp_Proc_Ptr->removeRepeatPathsLessSweepRadius(cells_sweeps, sweep_step / 2.0);

            // 移除清扫路径在障碍物中
            if(cells_sweeps.size() > 0)
                cells_sweeps = __ccpp_Proc_Ptr->removeObstacleSweepPaths(img, cells_sweeps);

            return cells_sweeps;
        }

        // 计算清扫路径
        std::vector<std::vector<Point_2>> BstphdnsPlanner::calcCellSweepPath(const Polygon_2 &poly, const FT offset, const Direction_2 &dir)
        {
            // Find start sweep.
            std::vector<std::vector<Point_2>> waypoints; // waypoints的元素就是一个个subtask
            bool counter_clockwise = true;
            Line_2 sweep(Point_2(0.0, 0.0), dir);
            std::vector<Point_2> sorted_pts = __polygon2d_Ptr->sortVerticesToLine(poly, sweep);

            const FT update_offset = offset;
            const FT kSqOffset = update_offset * update_offset;
            bool isEnd = false;
            sweep = Line_2(sorted_pts.front(), dir);
            // rotate sweep by 90 degree
            Vector_2 offset_vector = sweep.perpendicular(sorted_pts.front()).to_vector();
            offset_vector = update_offset * offset_vector /
                            std::sqrt(CGAL::to_double(offset_vector.squared_length()));
            const CGAL::Aff_transformation_2<K> kOffset(CGAL::TRANSLATION, offset_vector);

            std::pair<bool, std::vector<Segment_2>> sweep_segments = __polygon2d_Ptr->getSegments(poly, sweep); // 交点 -> 依次两两相连, 组成线段
            while (sweep_segments.first)
            {
                // 保存当前的清扫路线
                if (counter_clockwise && sweep_segments.second.size() > 1)
                {
                    std::reverse(sweep_segments.second.begin(), sweep_segments.second.end());
                }

                // 去除中间不可达
                if (sweep_segments.second.size() > 1)
                {
                    std::vector<Segment_2> tmp_sweep_seg;
                    for (size_t i = 0; i < sweep_segments.second.size(); i++)
                    {
                        const auto& seg_source = sweep_segments.second[i].source();
                        const auto& seg_target = sweep_segments.second[i].target();
                        Point_2 middle((CGAL::to_double(seg_source.x()) + CGAL::to_double(seg_target.x())) / 2.0,
                                       (CGAL::to_double(seg_source.y()) + CGAL::to_double(seg_target.y())) / 2.0);
                        if (__polygon2d_Ptr->pointInPolygon(poly, middle)) {
                            tmp_sweep_seg.emplace_back(sweep_segments.second[i]);
                        }
                    }
                    if (tmp_sweep_seg.size() > 0)
                        sweep_segments.second = tmp_sweep_seg;
                }

                // 最后的sweep_segments与之前的是否有交叉
                if (isEnd && waypoints.size() > 0 &&
                    sweep_segments.second.size() > 0 )
                {
                    // 去除重复的值
                    for (auto it = sweep_segments.second.begin(); it != sweep_segments.second.end();)
                    {
                        int sz = waypoints.size();
                        bool is_insection = false;
                        for (auto i = sz - 1; i >= 0; i--)
                        {
                            int sub_sz = waypoints[i].size();
                            for (auto j = sub_sz - 1; j >= 1; j--)
                            {
                                auto next_seg = Segment_2(waypoints[i][j], waypoints[i][j - 1]);
                                std::vector<Point_2> intersections;
                                typedef CGAL::cpp11::result_of<Intersect_2(Segment_2, Segment_2)>::type
                                    Intersection;
                                Intersection result = CGAL::intersection(*it, next_seg);
                                if (result)
                                {
                                    is_insection = true;
                                    break;
                                }
                            }
                            if (is_insection)
                                break;
                        }
                        if (is_insection)
                            it = sweep_segments.second.erase(it);
                        else
                            it++;
                    }
                    // 是否需要调整顺序
                    // 判断起始点
                    if(sweep_segments.second.size() > 0){
                        double dx = CGAL::to_double(sweep_segments.second.front().source().x()) -
                                    CGAL::to_double(waypoints.back().back().x());
                        double dy = CGAL::to_double(sweep_segments.second.front().source().y()) -
                                    CGAL::to_double(waypoints.back().back().y());
                        double dist1 = std::sqrt(dx * dx + dy * dy);
                        // 判断终点
                        dx = CGAL::to_double(sweep_segments.second.back().target().x()) -
                                    CGAL::to_double(waypoints.back().back().x());
                        dy = CGAL::to_double(sweep_segments.second.back().target().y()) -
                                    CGAL::to_double(waypoints.back().back().y());
                        double dist2 = std::sqrt(dx * dx + dy * dy);
                        if(dist1 > dist2) {
                            for(auto& sweep_segment : sweep_segments.second){
                                sweep_segment = sweep_segment.opposite();
                            }
                            std::reverse(sweep_segments.second.begin(), sweep_segments.second.end());
                        }
                    }
                }

                // 将当前的路径保存到way_points
                if (!isEnd && sweep_segments.second.size() > 0) {
                    for (auto i = 0; i < sweep_segments.second.size(); i++)
                    {
                        // Align sweep segment.
                        Segment_2 sweep_segment = counter_clockwise ? sweep_segments.second[i].opposite() : sweep_segments.second[i];
                        if (i == 0)
                        {
                            if (waypoints.size() == 0)
                            {
                                std::vector<Point_2> way_points;
                                way_points.emplace_back(sweep_segment.source());
                                way_points.emplace_back(sweep_segment.target());
                                waypoints.emplace_back(way_points);
                            }
                            else
                            {
                                waypoints.back().emplace_back(sweep_segment.source());
                                waypoints.back().emplace_back(sweep_segment.target());
                            }
                        }
                        else
                        {
                            std::vector<Point_2> way_points;
                            way_points.emplace_back(sweep_segment.source());
                            way_points.emplace_back(sweep_segment.target());
                            waypoints.emplace_back(way_points);
                        }
                    }
                }
                if(sweep_segments.second.size() > 0) sweep_segments.second.clear();

                // 结束直接退出
                if (isEnd)
                    break;
                // Offset sweep.
                sweep = sweep.transform(kOffset);
                // Find new sweep segment.
                sweep_segments = __polygon2d_Ptr->getSegments(poly, sweep);
                // Add a final sweep.
                if (!sweep_segments.first)
                {
                    // 取polygon中的最后一个slice
                    sweep = Line_2(*(sorted_pts.end() - 1), *(sorted_pts.end() - 2));
                    // sweep = Line_2(sorted_pts.back(), dir);
                    sweep_segments = __polygon2d_Ptr->getSegments(poly, sweep);
                    // 没切到polygon的不要
                    if (!sweep_segments.first)
                    {
                        // std::cout<<"Failed to calculate final sweep."<<std::endl;
                        break;
                    }
                    isEnd = true;
                }

                // Swap directions.
                counter_clockwise = !counter_clockwise;
            }
            return waypoints;
        }

        // 规划清扫区域顺序 从起点开始，欧式距离最近，贪心搜索，同时调整cell内的路点顺序
        std::deque<int> BstphdnsPlanner::sortCellsSweep(std::vector<std::vector<Point_2>> &cells_sweeps,
                                                        Point_2 start)
        {
            std::deque<int> cell_idx_path;
            Point_2 p_start = start;
            std::unordered_set<int> index_set;
            while (cell_idx_path.size() != cells_sweeps.size())
            {
                int starting_cell_idx = -1;
                double min_dist = DBL_MAX;
                for (size_t i = 0; i < cells_sweeps.size(); i++)
                {
                    if (index_set.find(i) != index_set.end())
                        continue;
                    std::vector<Point_2>::iterator iter;
                    if (cells_sweeps[i].size() <= 1)
                        continue;
                    double dist = __ccpp_Proc_Ptr->getCellSweepMinDist(p_start, cells_sweeps[i], iter);
                    if (min_dist > dist)
                    {
                        min_dist = dist;
                        starting_cell_idx = i;
                    }
                }
                // 把当前的cell索引添加到索引集中
                if (starting_cell_idx == -1)
                    break;
                __ccpp_Proc_Ptr->doReverseNextSweep(p_start, cells_sweeps[starting_cell_idx]);
                cell_idx_path.emplace_back(starting_cell_idx);
                index_set.insert(starting_cell_idx);
                p_start = cells_sweeps[starting_cell_idx].back();
            }
            return cell_idx_path;
        }

        // 优化弓字形
        std::vector<std::vector<Point_2>>
        BstphdnsPlanner::getOptimizedBoustrophedonPaths(bool& arc_boustrophedon,
                                                        float &sweep_width, float &sweep_interval,
                                                        std::deque<int> &cell_idx_path,
                                                        std::vector<std::vector<Point_2>> &cells_sweeps)
        {
            std::vector<std::vector<Point_2>> optimized_ways;
            // 优化cell生成的路径
            for (size_t i = 0; i < cell_idx_path.size(); ++i)
            {
                // has been cleaned?
                std::vector<Point_2> VecPt2;
                if (cells_sweeps[cell_idx_path[i]].size() > 1)
                {
                    if (cells_sweeps[cell_idx_path[i]].size() == 2)
                    {
                        auto dx = CGAL::to_double(cells_sweeps[cell_idx_path[i]][0].x()) - CGAL::to_double(cells_sweeps[cell_idx_path[i]][1].x());
                        auto dy = CGAL::to_double(cells_sweeps[cell_idx_path[i]][0].y()) - CGAL::to_double(cells_sweeps[cell_idx_path[i]][1].y());
                        if (std::sqrt(dx * dx + dy * dy) < 10)
                            continue;
                    }
                    // optimized_ways.emplace_back(cells_sweeps[cell_idx_path[i]]);
                    auto end = cells_sweeps[cell_idx_path[i]].begin();
                    auto start = cells_sweeps[cell_idx_path[i]].begin();
                    for (auto it = start + 1; it != cells_sweeps[cell_idx_path[i]].end();)
                    {
                        if ((it + 1) == cells_sweeps[cell_idx_path[i]].end())
                        {
                            VecPt2.clear();
                            VecPt2.insert(VecPt2.end(), start, it + 1);
                            if(VecPt2.size() == 2){
                                auto dx = CGAL::to_double(VecPt2[0].x()) - CGAL::to_double(VecPt2[1].x());
                                auto dy = CGAL::to_double(VecPt2[0].y()) - CGAL::to_double(VecPt2[1].y());
                                if (std::sqrt(dx * dx + dy * dy) > 10) optimized_ways.emplace_back(VecPt2);
                            }else if(VecPt2.size() > 2){
                                optimized_ways.emplace_back(VecPt2);
                            }
                            break;
                        }
                        auto dx = CGAL::to_double(it->x()) - CGAL::to_double((it + 1)->x());
                        auto dy = CGAL::to_double(it->y()) - CGAL::to_double((it + 1)->y());
                        if (std::sqrt(dx * dx + dy * dy) > (sweep_width / sweep_interval) * std::sqrt(2))
                        {
                            VecPt2.clear();
                            VecPt2.insert(VecPt2.end(), start, it + 1);
                            if(VecPt2.size() == 2){
                                auto dx = CGAL::to_double(VecPt2[0].x()) - CGAL::to_double(VecPt2[1].x());
                                auto dy = CGAL::to_double(VecPt2[0].y()) - CGAL::to_double(VecPt2[1].y());
                                if (std::sqrt(dx * dx + dy * dy) > 10) optimized_ways.emplace_back(VecPt2);
                            }else if(VecPt2.size() > 2){
                                optimized_ways.emplace_back(VecPt2);
                            }
                            start = it + 1;
                        }
                        it = it + 2;
                    }
                }
            }
            // 优化圆弧弓字形
            if (arc_boustrophedon) optimized_ways = getArcBoustrophedonPath(sweep_width, sweep_interval, optimized_ways);
            return optimized_ways;
        }

        // 获取单个cell相邻圆弧路径
        std::vector<Point_2> BstphdnsPlanner::getOneCellAdjArcPath(int lenth, float interval_radian, float adj_dx, float adj_dy,
                                                                   float arc_radius, float dx, float dy, float mid_x, float mid_y)
        {
            std::vector<Point_2> arc_result;
            auto theta = std::atan2(dy, dx);

            if (dy > 1e-3)
            {
                if (theta < 0)
                    theta = M_PI + theta;
                // theta对应以arc_radius为半径构成圆的坐标
                auto tmp_y = arc_radius * sin(theta);
                auto tmp_x = arc_radius * cos(theta);

                float tmp_theta;
                float factor = 1.0;
                // 角度变化theta + lenth * interval_radian  ---> theta - lenth * interval_radian
                if (adj_dx > 0)
                {
                    tmp_theta = theta + lenth * interval_radian;
                    factor = -factor;
                } // 角度变化theta - lenth * interval_radian  ---> theta + lenth * interval_radian
                else
                {
                    tmp_theta = theta - lenth * interval_radian;
                }

                for (auto i = 0; i < lenth * 2; i++)
                {
                    float x, y;
                    // tmp_theta对应以arc_radius为半径构成圆的坐标
                    auto tmp_x1 = arc_radius * cos(tmp_theta + factor * i * interval_radian);
                    auto tmp_y1 = arc_radius * sin(tmp_theta + factor * i * interval_radian);
                    // 对比theta与tmp_theta对应的x/y值，转化为实际值
                    if (tmp_y > tmp_y1)
                        y = mid_y - (tmp_y - tmp_y1);
                    else
                        y = mid_y + (tmp_y1 - tmp_y);
                    if (tmp_x > tmp_x1)
                        x = mid_x - (tmp_x - tmp_x1);
                    else
                        x = mid_x + (tmp_x1 - tmp_x);
                    arc_result.emplace_back(Point_2(x, y));
                }
            }
            else if (dy < -1e-3)
            {
                if (theta > 0)
                    theta = theta - M_PI;
                // theta对应以arc_radius为半径构成圆的坐标
                auto tmp_y = arc_radius * sin(theta);
                auto tmp_x = arc_radius * cos(theta);

                float tmp_theta;
                float factor = 1.0;
                // 角度变化theta - lenth * interval_radian  ---> theta + lenth * interval_radian
                if (adj_dx > 0)
                {
                    tmp_theta = theta - lenth * interval_radian;
                } // 角度变化theta + lenth * interval_radian  ---> theta - lenth * interval_radian
                else
                {
                    tmp_theta = theta + lenth * interval_radian;
                    factor = -factor;
                }

                for (auto i = 0; i < lenth * 2; i++)
                {
                    float x, y;
                    // tmp_theta对应以arc_radius为半径构成圆的坐标
                    auto tmp_x1 = arc_radius * cos(tmp_theta + factor * i * interval_radian);
                    auto tmp_y1 = arc_radius * sin(tmp_theta + factor * i * interval_radian);
                    // 对比theta与tmp_theta对应的x/y值，转化为实际值
                    if (tmp_y > tmp_y1)
                        y = mid_y - (tmp_y - tmp_y1);
                    else
                        y = mid_y + (tmp_y1 - tmp_y);
                    if (tmp_x > tmp_x1)
                        x = mid_x - (tmp_x - tmp_x1);
                    else
                        x = mid_x + (tmp_x1 - tmp_x);
                    arc_result.emplace_back(Point_2(x, y));
                }
            }
            else
            {
                if (adj_dy > 1e-3)
                {
                    // theta对应以arc_radius为半径构成圆的坐标
                    auto tmp_y = arc_radius * sin(theta);
                    auto tmp_x = arc_radius * cos(theta);
                    // 角度变化theta - lenth * interval_radian  ---> theta + lenth * interval_radian
                    auto tmp_theta = theta - lenth * interval_radian;
                    float factor = 1.0;
                    for (auto i = 0; i < lenth * 2; i++)
                    {
                        float x, y;
                        // tmp_theta对应以arc_radius为半径构成圆的坐标
                        auto tmp_x1 = arc_radius * cos(tmp_theta + factor * i * interval_radian);
                        auto tmp_y1 = arc_radius * sin(tmp_theta + factor * i * interval_radian);
                        // 对比theta与tmp_theta对应的x/y值，转化为实际值
                        if (tmp_y > tmp_y1)
                            y = mid_y - (tmp_y - tmp_y1);
                        else
                            y = mid_y + (tmp_y1 - tmp_y);
                        if (tmp_x > tmp_x1)
                            x = mid_x - (tmp_x - tmp_x1);
                        else
                            x = mid_x + (tmp_x1 - tmp_x);
                        arc_result.emplace_back(Point_2(x, y));
                    }
                }
                else if (adj_dy < 1e-3)
                {
                    // theta对应以arc_radius为半径构成圆的坐标
                    auto tmp_y = arc_radius * sin(theta);
                    auto tmp_x = arc_radius * cos(theta);
                    // 角度变化theta + lenth * interval_radian  ---> theta - lenth * interval_radian
                    auto tmp_theta = theta + lenth * interval_radian;
                    float factor = -1.0;
                    for (auto i = 0; i < lenth * 2; i++)
                    {
                        float x, y;
                        // tmp_theta对应以arc_radius为半径构成圆的坐标
                        auto tmp_x1 = arc_radius * cos(tmp_theta + factor * i * interval_radian);
                        auto tmp_y1 = arc_radius * sin(tmp_theta + factor * i * interval_radian);
                        // 对比theta与tmp_theta对应的x/y值，转化为实际值
                        if (tmp_y > tmp_y1)
                            y = mid_y - (tmp_y - tmp_y1);
                        else
                            y = mid_y + (tmp_y1 - tmp_y);
                        if (tmp_x > tmp_x1)
                            x = mid_x - (tmp_x - tmp_x1);
                        else
                            x = mid_x + (tmp_x1 - tmp_x);
                        arc_result.emplace_back(Point_2(x, y));
                    }
                }
            }
            return arc_result;
        }

    } // namespace ccpp
} // namespace global_path_planner